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1 
NAVIGATION SYSTEM 


TECHNICAL FIELD 


This invention relates to a navigation system, and 
more particularly to a system for locating the position 
of a vehicle, movable over a surface which varies in 
elevation, as it moves along a navigation path. 


BACKGROUND ART 


‘In the past several years, tremendous strides forward 
have been taken in the field of navigational dead reck- 
oning techniques. Among recent developments in this 
field have been the introduction of computers and the 
development of velocity, acceleration, and direction 
sensing devices of high accuracy. However, despite the 
tremendous advances made in dead reckoning guidance 
systems employed for bringing a craft or vehicle pre- 
cisely to a certain geographic location, a fix-taking cor- 
rectional guidance system must still be used in conjunc- 
tion with the dead reckoning system because of the 
characteristic accumulation of dead reckoning error in 
the latter, if high accuracy of navigation is required. 

Generally speaking, the reference data necessary for 
use in a correctional system can be derived by several 
techniques and from a variety of sources. Two common 
methods use celestial observation and the recognition of 
some earth-fixed parameter. While stellar monitoring 
can usually be satisfactorily employed at high altitudes, 
several factors prevent its use in high-speed, low-alti- 
tude vehicles. First, weather and cloud cover impose 
operational limitations in land and air vehicles and in 
vessels operating at and near the surface of water. Se- 
condly, a turbulent boundary layer is formed during 
low and medium altitude flights of aerial vehicles which 
cause image diffusion and defraction and therefore a 
corresponding degradation in accuracy. Obviously, 
optical observation of stellar bodies is not readily practi- 
cable, in the case of a vessel traveling deep beneath the 
surface of a body of water, for providing stellar refer- 
ence data for fix-taking. 

One earth fixed parameter data source is topographic 
information. Many guidance systems are devised in the 
past which, at least in aircraft, made use of topographic 
information as reference data for fix-taking. Some of 
these systems made use of radar derived topographic 
data, and large efforts were expended in developing 
radar map matching techniques. Systems of this type 
have been in existence for some years, but have never 
been completely satisfactory because of, primarily, their 
high degree of complexity. 

It will be understood that, as employed herein, the 
term “navigation” refers to the conducting of aircraft 
and ships from place to place and further is intended to 
refer, and expressly does refer, to the conducting of any 
other body from place to place. Thus, while the specific 
example provided herein is in connection with an air- 
craft as the vehicle, the sequence of elevations, relative 
to some fixed reference, from one to the other along a 
given series of discrete points on the ocean bottom is as 
unique as along a similar series of points on land, and the 
elevation sequence along a series of spaced points on 
land is no less unique when the points are passed over 
by a land-contacting vehicle than when flown over by 
an aircraft. The invention, therefore, is specifically ap- 
plicable also to the navigation of submarine vessels and 
land vehicles and, in fact, of any body which moves 
over a surface, the earth’s crust being one example 
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thereof, whose altitude varies from place to place with 
reference to a given altitude datum. While, in the spe- 
cific example, altimeters are referred to as preferred 
means for determining both the absolute altitude of an 
aircraft relative to a reference datum and the height of 
the aircraft above the earth, the invention is by no 
means limited to the use of such instruments and its 
scope is such as to include, in other applications, the use 
of fathometers and/or pressure-sensing devices giving 
information indicative of the altitude of the earth’s crust 
and specifically the interval separating a vessel from the 
ocean bottom and/or surface. 

While the term “terrain” ordinarily has been em- 
ployed, in the past, with references to land areas, it is 
expressly adopted and employed herein as a term refer- 
ring to any surface area, such as that of the earth’s crust, 
whether that area be covered with water or air. 

Previously proposed fix-taking and navigational sys- 
tems have sought to utilize terrain elevation data, and 
they have been based upon the analog comparison of 
sample data, which are the continuous, analog represen- 
tation of continuous variations in terrain elevations, 
with similar data contained in contour maps employed 
as such. At least some of the sample and known data 
hence have always been graphically or photographi- 
cally displayed on actual sheets of paper, rectangles of 
photographic film, etc., and the values represented 
thereby have been shown as physically measurable 
along at least two axes. Because of the nature of the data 
employed, cumbersome and unwieldy equipments for 
photographic development, superposition of map over 
map, orthogonal adjustments of one set of data relative 
to another, etc. have been unavoidable sources of added 
weight, complexity, error and malfunction. 

One significant improvement of such analog compari- 
son systems is described in U.S. Pat. No. 3,328,795 
which does not employ continuously recorded, analog 
data, but has as one of its bases the use of quantized 
terrain altitude information taken at discrete points. A 
numerical comparison of sample and prerecorded data 
is performed at high speed, and with results predictable 
and repeatable for the same inputs, by a digital com- 
puter. Since the digital computer and associated compo- 
nents are relatively unaffected by noise, vibrations, 
nuclear radiation, etc., no equipment is required for 
performing two-dimensional data comparisons, and no 
feedback or nulling circuitry is needed for determining 
the point of best physical correlation of the sample with 
the prerecorded data. As distinguished from systems 
utilizing analog information, the digital computer is free 
from the sources of error unavoidably present where 
analog comparisons are made and hence is not only 
more accurate but is able to tolerate relatively large 
errors in sample and known data values without com- > 
promising fix-taking accuracy. 

Basically the navigation system as described in U.S. 
Pat. No. 3,328,795 involved the lifting of the contour 
signature of the prospective navigation region or re- 
gions, from available sources such as contour maps or 
stereo photos. As a vehicle proceeds over the naviga- 
tion region, a sensor system measures the contour signa- 
ture of a terrain sample along the vehicle navigation 
track. As described in U.S. Pat. No. 3,328,795, the ter- 
rain sample is nominally five miles in length to obtain 
uniqueness and it can be curved or straight. The sample 
of the measured contour signature from along the navi- 
gation track is trial-matched with stored terrain signa- 
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tures of similar samples from all over the navigation 
region. The objective of the matching is to determine 
the location within the navigation region from which 
the sample of measured navigation track signature was 
taken. When a best match is determined, the navigation 
system of U.S. Pat. No. 3,328,795 position fixes within 
the region and the system updates a dead reckoning 
navigation subsystem in appropriate coordinates. 

An improvement over the navigation system of U.S. 
Pat. No. 3,328,795 is described in U.S. Pat. No. 
4,144,571 and utilizes an update of vehicle position data 
on a point by point basis. At each terrain measurement, 
the vehicle position and vehicle velocity data are up- 
dated such that the updated data is equal to the previous 
data, plus the movement of the vehicle due to a mea- 
sured velocity, plus movement due to a bias in the ve- 
locity measurement, plus a characteristic error term. In 
computing the updated data in U.S. Pat. No. 4,144,571, 
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absolute altitude above a reference are compared with 
stored reference data at the predicted vehicle position. 
As distinguished from a terrain path comparison tech- 
nique, three vehicle position measurements and three 
vehicle velocity measurements are updated at each data 
sample using recursive computation techniques based 
on past measurement data. 

An ideal fix-taking navigation system should possess 
operational flexibility and should perform satisfactorily 
where nuclear radiation or other adverse environmental 
and/or flight condition exist. Moreover, the system 
should preferably possess the attributes of simplicity, 
accuracy, and reliability and desirably should be com- 
pact and light-weight. 

A fix-taking navigation system should possess addi- 
tionally the ability to operate independently of the lin- 
earity or nonlinearity of the vehicle path and without 
previous knowledge of the vehicle movement as this 
may unexpectedly vary in flight. 


DISCLOSURE OF THE INVENTION 


In accordance with the present invention, apparatus 
for determining the path of a vehicle over a given area 
as represented by a stored data terrain elevation map 
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position and velocity of the vehicle as it moves over the 
area represented by the stored map. A terrain clearance 
estimator responds to terrain elevation signals and gen- 
erates output data representing vehicle clearance above 
the terrain covered by the stored map. The terrain ele- 
vation map is stored as a continuous function in rectan- 
gular coordinates and is accessible to a computational 
processor that also responds to the output data from the 
navigation system and output data from the terrain 
clearance estimator. The computational processor gen- 
erates vehicle velocity correction estimates and vehicle 
position correction estimates which are accessible by 
the navigation system to generate corrected vehicle 
navigation signals for determining the path of the vehi- 
cle to a destination. 


BRIEF DESCRIPTION OF THE DRAWINGS 


A more complete understanding of the invention and 
its advantages will become apparent from the following 
description when read in light of the accompanying 
drawings. 

Referring to the drawings: 


50 


55 


65 


4 


FIG. 1 is a block diagram of one embodiment of a 
vehicle navigation system in accordance with the pres- 
ent invention; 

FIG. 2 is a block diagram showing one implementa- 
tion of a leading edge estimator for the system of FIG. 
1; 

FIG. 3 is a block diagram of an implementation of a 
clocking and timing circuit for the timer of FIG. 2; 

FIG. 4 is a functional diagram of the computational 
processor of FIG. 1; and 

FIG. 5 is a flow chart of the error estimation algo- 
rithm for generating position correction and velocity 
correction signals for the inertial navigation system of 
FIG. 1 in accordance with the functional diagram of 
FIG. 4. i 


DETAILED DESCRIPTION 


Vehicle navigation in accordance with the present 
invention is accomplished by a system that determines 
the velocity and position of a vehicle with respect to a 
sequence of reference data representing a reference map 
hz(x,y) that may be stored in a number of ways. Hereto- 
fore, the reference map was stored as a rectangular 
array which can be described as a two-dimensional bar 
graph. The stored discrete data items relate to varia- 
tions in elevation of the terrain relative to a reference 
datum. The individual discrete data items are stored as 
the mathematical function h;(x,y) and represent mea- 
surements of elevation variation of a surface area and 
form a unique pattern to identify the velocity and posi- 
tion of a vehicle along a navigation track. There are 
alternate ways of utilizing this stored data, one utilizing 
a method of interpolation to evaluate the terrain be- 
tween grid points and the other using no interpolation. 
Use of interpolation improves the accuracy of the out- 
put data. Another technique for computing velocity and 
position of a vehicle is to store the terrain map as a 
mathematical function. This is the technique that will be 
described hereinafter which enables construction of the 
reference map Һ.(х,у) as the sum of orthogonal func- 
tions. The functional form used is a discrete cosine 
transform (DCT) as given by the expression: 


M 
пу) = mo n 


(1) 


N 
2 2 bmn cos(mwxx)cos(nwyy) 


This functional form enables the generation of a wide 
variety of “terrain-like” surfaces. The accuracy desired 
when utilizing the expression above varies with the 
number of terms in the equation. By utilizing this tech- 
nique, data representing the entire rectangular grid need 
not be stored but instead it is only required to store the 
coefficients of the sum of the orthogonal function. 
These coefficients (bmn) are computed from at least 
squares fit of the expression hz(x,y) to the rectangular 
grid elevation data provided by distance measuring 
apparatus. Thus instead of storing many terrain eleva- 
tion data points in a rectangular grid it is only necessary 
to store 20 or 30 values of the coefficients bmn and com- 
pute the terrain map function h,(x,y) whenever it is 
needed to compute the terrain elevation at a point (x,y). 

Referring to FIG. 1, there is shown one embodiment 
of a navigation system including a conventional radar 
terrain sensor 10 connected through an antenna steering 
control 12 to a conventional radar antenna 14. As a 
vehicle enters a reference field the radar terrain sensor 
10 begins to transmit and receive pulses which are pro- 
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cessed into video output signals that are applied to a 
terrain clearance estimator 16. The terrain clearance 
estimator 16 processes the video output signals (returns) 
to generate output data representing a running account 
of terrain clearance with respect to a reference drum, 
1.е., a vehicle. 

Vehicle position is measured by an inertial navigation 
system 18 including a navigation function controller 20 
and a TFS attitude sequencing control 22. The attitude 
sequencing control 22 generates roll steering commands 
and update commands to the antenna steering control 
12 for positioning of the antenna 14. At periodic inter- 
vals the vehicle position as measured by the inertial 
navigation system 18 is applied to a buffer 24 as part of 
a computational processor 26. Altitude signals and ver- 
tical velocity signals from the sequencing controller 22 
from the inertial navigation system are input to the 
terrain clearance estimator 16. The output of the terrain 
clearance estimator 16 is sampled periodically along 
with the output of the navigation function controller 20 
and stored in the buffer 24. By the sampling technique 
the vehicle position samples and the terrain clearance 
estimator samples are stored as signal “pairs” in the 
buffer 24. 

With reference to the inertial navigation system 18, 
such a device is more thoroughly described in U.S. Pat. 
No. 4,144,571 and generates steering commands to a 
conventional control unit. Typically, the control unit 
may be a conventional autopilot for providing steering 
to a vehicle along a desired navigation path. Because of 
the conventional aspects of the navigation system 18 
further detail of such system is not considered necessary 
for a full understanding of the present invention. 

Data “pairs” in the buffer 24 are utilized by the com- 
putational processor 26 in a correlation process which 
begins at the end of a subsequent data run. As illus- 
trated, the computational processor 26 includes the 
reference map function Һ.(х,у) data storage 28 which 
contains prerecorded terrain coefficient data. This coef- 
ficient data, which is output from the storage 28 as a 
function of vehicle position in Cartesian coordinates is 
input along with a data pair from the buffer 24 to a 
position and velocity error estimator 30. By means of an 
iterative process the error estimator 30 updates values 
of position and velocity error estimates for a vehicle 
with each iteration stored in a data register 32. These 
updated values are then accessed by the inertial naviga- 
tion system 18 on an as-needed basis. Typically, the 
estimator will achieve acceptable accuracy of position 
and velocity correction after approximately 20 itera- 
tions although it continues to function in order to obtain 
greater accuracy until a vehicle reaches an identified 
destination, 

Operationally, the system of FIG. 1 becomes opera- 
tional by initializing the inertial navigation system 18 at 
the time the vehicle leaves a starting or take-off point. 
Assuming that the take-off point is known without error 
then at the starting time, to, the output of the inertial 
navigation system 18 may be assumed to be without 
error. However, errors build up in the inertial naviga- 
tion system as the vehicle follows its assigned path. As 
a result, by the time the vehicle arrives in the vicinity of 
its destination an unacceptable uncertainty in the posi- 
tion and velocity of the vehicle as measured by the 
inertial navigation system has developed. In accordance 
with the present invention, the system of FIG. 1 cor- 
rects this error by taking observations of the terrain 
within the specified area defined by the reference map 
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function in the storage 28 prior to the arrival of the 
vehicle at its destination. These observations are pro- 
cessed to generate position correction estimates, Af, and 
velocity correction, Af which are accessed by the iner- 
tial navigation system to provide corrected position and 
velocity signals for the inertial navigation system out- 
put. 

For purposes of further description, assume that the 
path of the vehicle from its starting position to its desti- 
nation can be divided into three time segments: (to, t1), 
(ti, tk), and (tk, t), where: 

to is the starting time, ? 

tı is the time at which the first observation of the 

terrain is taken as the vehicle approaches its desti- 
nation, 

tx is the time at which the final terrain observation is 

taken before the vehicle reaches its destination, and 
tris the time at which the vehicle arrives at its destina- 
tion. 

Typically, the time interval from to to 11 is very much 
larger (several thousands of seconds) than the interval 
tı to tr(which may be less than 15 or 20 seconds). Thus 
it may be assumed that approximately all the error built 
up in the inertial navigation system 18 occurs prior to 
time 11, and that the error built up in the interval from 
time t; to time tris relatively small. 

This may be more precisely defined if the vehicle 
destination is used to specify the origin of a Cartesian 
coordinate system in which it is desired to know the 
position and velocity of a vehicle. If the terms r(t) and 
i(t) represent the actual position and velocity, respec- 
tively, of a vehicle at time t in this coordinate system 
and the position and velocity of the vehicle as measured 
by the inertial navigation system are given by the terms 
u(t) and p(t), respectively, and if it is further assumed 
that if the vehicle position at the starting point is known 
without error, then at the starting time, to, the output of 
the internal navigation system is error free and is given 
by the expressions: 


Ito) = r(to), and Q), 


Bo) = (to). (3) 

As the vehicle moves along its assigned path to its 
destination errors are built up in the output of the iner- 
tial navigation system 18 so that at time t; a position 
error, Ar, and a velocity error, Ar, will be present as 
given by the expressions: 


Ar—r(n)— p(t), and (4) 


Аг= қ) p(t). (5) 

Proceeding оп the assumption that the error accrued 
to the inertial navigation system during the interval 
from time t; to tris relatively small, then during this 
interval the actual position and velocity of a vehicle is 
related to the output of the inertial navigation system by 
the equations: 


қ) = Ar Аң) (6) 


Қд-і()--А? (7) 

These equations reveal that the corrected position 
and velocity of the vehicle (r(t) and r(t)) can Бе com- 
puted independently of the form of u(t) and (4), that is, 
independent of the "shape" of the trajectory as indi- 
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cated by the navigation system. Hence, there is no limi- 
tation that the vehicle path be linear. Also, it is not 
necessary to alter the method for constructing the cor- 
rected navigation signals for curvilinear paths. 

With reference to these two equations the computa- 
tional processor 26 provides the position error, Ar, and 
the velocity error, Ar, in the data register 32 for access 
by the inertial navigation system. Thus, the computa- 
tional processor 26 outputs two three-dimensional vec- 
tors defining Ar and Ar as input to the data register 32. 
'The computational processor 26 generates these vectors 
given the terrain elevation map as stored in storage 28, 
a terrain clearance history from the clearance estimator 
16 and position output signals from the inertial naviga- 
tion system 18. 

Referring to FIG. 2, there is shown a block diagram 
of a leading edge estimator as part of the terrain clear- 
ance estimator 16 wherein the clearance of the terrain at 
any time is determined from a running account of the 
time of arrival data using the relationship between dis- 
tance and time characteristics of the radar system in- 
cluding the sensor 10. The function of the leading edge 
estimator of FIG. 2 is to track the earth's surface by 
estimating the times of arrival of the radar returns to the 
sensor 10. The input to the time of arrival estimator on 
a line 104 is the video output of the radar terrain sensor 
10. The video output of the sensor 10 is fed through a 
delay line 106 and differenced in a summing junction 
108 to obtain a continuous output that is applied to a 
sample and hold network 110. The quantity in the sam- 
ple and hold circuit 110 is accessed by an enable signal 
from a timer 112 that is in turn enabled by the output of 
a pulse repetition interval generator 114. The pulse 
repetition interval generator 114 enables the timer 112 
which runs for a preselected number of seconds at 
which time it sends the enable signal to the sample and 
hold circuit 110. The value in the sample and hold net- 
work 110 is applied to a one bit analog-to-digital con- 
verter 116 to determine the sign of the value. The out- 
put of the analog-to-digital converter 116 then acts as an 
enable pulse to determine whether a ** —b" signal on line 
118 or a “+f” signal on line 120 is fed to an adder 122 
to be summed with a previous time of arrival estimate 
signal as stored in a register 124. The new estimate of 
the time of arrival is then stored in the register 124 and 
replaces the previously stored value. The estimate in the 
register 124 is accessed as needed for processing with 
the output of the inertial navigation system 18 as sam- 
pled and stored in the buffer 24. 

Referring to FIG. 3, there is shown a block diagram 
of one implementation of the timer 112 of the time of 
arrival estimator of FIG. 2. The time of arrival estima- 
tor of FIG. 2 includes a conventional sample and hold 
network, an analog-to-digital converter, an adder, and a 
register along with a PRI generator and delay line. 
Timer 112, as detailed in FIG. 3, functions by receiving 
as an input from the PRI generator 114 an enable signal 
on a line 126. This enable signal actuates a clock 128 
having an output applied to a tapped delay line 130 and 
to an up-down counter 132. Each output terminal of the 
delay line 130 is tied to a multiplexer 134 that also is 
connected to a multiplexer 136. The multiplexer 136 in 
turn connects to the up-down counter 132 and receives 
as an input on a line 138 the most recent estimate of time 
of arrival value as stored in the register 124 of FIG. 2. 

Connected to the output of the multiplexer is an 
AND gate 140 having a second input connected to a 
NOR gate 142 with inputs connected to the up-down 
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8 
counter 132. By operation of the AND gate 140 an 
enable signal is generated on a line 144 to enable the 
sample and hold network 110 of FIG. 2. 

Operationally, the timer of FIG. 3 receives an enable 
pulse from the PRI generator 114 and accesses the reg- 
ister 124 for the most recent estimate of the time of 
arrival. With these two outputs the timer generates the 
enable signal to sample the value of the differenced 
video radar return stored in the network 110. Operation 
of the sample and hold network 110 in the time of ar- 
rival estimator has been previously described with ref- 
erence to FIG. 2. 

Turning next to the radar terrain sensor operation 
along with the time of arrival estimator of FIG. 2 in the 
terrain clearance estimator 16 to generate observed 
terrain clearances it will be assumed that as a vehicle 
moves over the reference map field a total of К mea- 
surements of the terrain (K may nominally be 100 sam- 
ples) are taken. Considering a nominal pulse repetition 
frequency for the terrain sensor 10 of 10 KHz and a data 
run of typically 2.5 seconds, then a total of 25,000 radar 
returns from the terrain are obtained by the sensor 10. 
An observation of terrain clearance is made by measur- 
ing the time of arrival of a return pulse in the time of 
arrival estimator of FIG. 2. If the estimated time of 
arrival of the nth pulse transmitted at time t, is denoted 
to by тд Пеп the observed terrain clearance is computed 
in the estimator 16 in accordance with the expression: 


^ 


C 7, 8 
em n= l.. 25000 e 


Үй) = 


where: 

Y(t5)-a value of terrain clearance, and 

C is the speed of light. 

If only 100 observations (samples) of terrain clear- 
ance are to be used by the processor 26 to estimate 
position error, Ar, and velocity error, Ar, and assuming 
these observations are equally spaced in time, then the 
terrain clearance estimator 16 provides an output to the 
buffer 24 in accordance with the expression: 


C ?250к (9) 


Үйт) = 5 


This expression relies on the time of arrival estimate as 
stored in the register 124 which has been generated in 
accordance with the expression: 


2+1 =( 


where а weighting function has been incorporated as 
evidenced by the constants “—Ъ” and “+f” as input to 
the gating amplifier 121. 

Referring to FIG. 4, there is shown a hardware im- 
plementation of the computational processor 26 which 
at the completion of a data run for storing data in the 
buffer 24 begins an iterative operation to update values 
of the position and velocity error estimates (Af, А2). 
Basically, the computational processor 26 is a 16 bit 
(word size) mini-computer capable of performing the 
arithmetic of an algorithm to be described with refer- 
ence to FIG. 5. In one implementation of the system of 
the present invention, the computational processor 26 
includes a National Semiconductor IMP-16C mini-com- 


^ Я ^ А (10) 
Tn — b, if Sn4 14) < 5+1 + 8) 


75 + f; otherwise 
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puter which is a general purpose 16 bit machine that 
performs 2’s complement arithmetic. This mini-com- 
puter is microprogrammable and performs double word 
arithmetic. The design uses mostly basic TTL inte- 
grated circuits with MOSLSI chips for the registers and 
arithmetic logic unit and the control read only memory. 
System power is applied to clear logic 146 to control a 
clock and timing circuit 148 that receives system clock 
pulses on a line 150. Also input to the clock and timing 
circuit 148 is an INS control signal on a line 152. 

Clock signals generated by the clock and timing cir- 
cuit 148 are provided to ROMs and timing control 154, 
process control logic 156, status control 158, a memory 
cycle register 160, and MDR logic 162. Control signals 
for the MDR logic 162 are also provided to the process 
control logic 156 and in addition are applied to a shift 

` register 164, bus control logic 166, accumulator regis- 
ters 168 and 170, and a pointer controller 172. In addi- 
tion, the control signals from the MDR logic 162 are 
applied to a storage register 174 and MAR logic 176. 

Control signals from the memory cycle register 160 
are input to a memory timing register 178 which gener- 
ates timing pulses to a memory ROM 180 and a memory 
RAM 182. Data is transferred between various compo- 
nent parts of the computational processor 26 during 
operation thereof on data busses 184, 186 and 188. Data 
is also transferred between an external register 190 and 
an inertial navigation system data input/output register 
192. The INS register 192 also receives the INS control 
signals on the line 152 which are also applied to the 
ROMs and timing control 154. 

Referring to FIG. 5, operation of the computational 
processor 26 will be explained with reference to the 
detailed flow chart where a computation begins at the 
completion of a data run at a start sequence 194. The 
system is initialized at 196 and an instruction is run at 
198 to test if the sequence is to stop with a positive 
response or proceed to an update position and velocity 
sequence 200. The stop sequence is illustrated at 202. 

Because the position error of the inertial navigation 
system is expected to be much larger than the velocity 
error the frist few iterations of the operation of the 
computational processor 26 will be to minimize the 
position error. Thus at the inquiry sequence 200 the first 
response will be a position iteration. 

Prior to detailing the position or velocity error com- 
putations from the flow chart of FIG. 5 a description of 
the position and velocity error estimation algorithm 
will be presented. Estimate of the position and velocity 
errors, Af and Af, are calculated to minimize the expres- 
sion: 


А K x 11 
RAF, А) = > [000 – Yao? ne 


where: 
Y(tx) is the observed terrain clearance at time tz, and 
Y(tx) is the predicted value of the terrain clearance at 
time t; for the position and velocity errors given by 
Af and Af. М 
From the above the predicted terrain clearance, Y(tx) 
is provided in accordance with the expression: 


Ў) 2209 ҺА), Ха) (12) 


where 
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(13) 
= Яд = u(t) + AP + AKE — t), 


ht) is the sensor viewed “terrain elevation” com- 
puted at the nominal vehicle altitude z, and 
u(t) is the vehicle position at time t as measured by the 
inertial navigation system 18. 
With appropriate substitutions the expression for 
R(Af, AT) becomes: 


(АРАУ = (14) 


K А 
24 UB — 200 — Az — АЕ — DAr + AADI 


| 


where 


х(9 
u(t) = | уф 
z() 


th = (К — DA: and tj = 0. 


This is a six-dimensional minimization of R(Ar, Af) 
which can be reduced to a four-dimension minimiza- 
tion, namely, Ax, Ay, AX and Ay. 

Using equation (14) the derivatives are set equal to 
zero so that: 


(15) 
2K — 1 


AzB) = eR [o5 оф - s | 


12 kc Kail 
= -ik DKE F i [so - 2 


z) 


K 
Ше i Yt) — 2'(th) = аа) 


(16) 
оф | 


(7) 


and 


А K à (18) 
58) = > 1 (k — ПУ — z à) + ALEID] 


Backsubstituting these expressions for AZ and AZ into 
equation (14) yields the modified residual (В) as: 
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А K : (19) 
48) = 2 [И — 200 — А) — 


(k — DAr zB) + AO UP. 


To minimize В(А?, Ai) in АҒ, Д? it is now sufficient to 
minimize only (8) in B. Thus a six dimensional minimi- 
zation has been reduced to a four dimensional minimiza- 
tion. 

This four dimensional minimization is accomplished 
in the computational processor 26 iteratively so that a 
sequence of estimates m, т=0, 1, 2, 3,... will be 
computed and made accessible to the inertial navigation 
system 18 along with Д2(8) and А2(В,) for each m. 
The initial estimate, Bo, of the position and velocity 
errors in the xy-plane is taken as 0 for each component 
of error. Each successive value of B is obtained by 
implementing the recursive relation: 

Bm+1=Bmtém+1 - (20) 
where the four dimensional correction e, , 1 is specified 
below. И 

The four dimensional minimization of ХВ) is further 
broken down into two interrelated minimizations, one 
of which is in the position plane, (Ax,A9), and the other 
in the velocity plane, (Ax, Ay). The minimization algo- 
rithm operates by alternately minimizing (В) through 
first a position error minimization and the velocity error 
minimization. Thus the correction term, ет--1, has two 
different forms correcting for position and velocity 
error depending upon which error is being estimated at 
iteration m+1. 

The position error in the inertial navigation system 18 
is expected to be much larger than the velocity error so 
the first few iterations of the flow chart of FIG. 5 will 
be a position error minimization. Position error minimi- 
zation is accomplished by letting: 


| (21) 
схт-+ 1581 [J(3m) — Bm + 8%m+1)] 
сут-- 1581 КВт) — МВт + 8¥m+1)] 
0 
0 


€m+1 = 


where 


СХт, Сут, dXm, dym, m=1, 2, 3, . . . are scalar sequen- 
ces which decrease as m increases, 
and the function sgn [ ] is defined as 


sgn{x] = ( 


The correction factor for the mth iteration in position 
is obtained by examining the modified residual for three 
values of В. Depending on the comparative magnitudes 
of the modified residual at 8, and Ви-- бхт--1 the Ax 
coordinate of the correction term €m+1 is either 
+CXm+1 ОГ —CXm+ 1. Likewise for the Ay coordinate. 


+lifx>0 
— 1 otherwise. 
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As a result of examining these three values of B the 
estimate В» + 1 can take on one of the four values. 

The sequences сх», and Cym are chosen to steadily 
decrease so that the increment of correction decreases 
to insure that Ви eventually converges to the global 
minimum for ХВ). Each correction € is successively 
"finer", as m increases. There are several methods for 
selecting these two sequences. The method employed is 
to count the number of sign changes, Nx(m) and N,(m), 
in the increments of corrections and compute сх» and 
сут аз: 


схт+1=2—МтТехо (22) 


and 


CYm4- 1227 М") суд (23) 
The rational for doing this is that when the initial esti- 
mate Во is very much different from the true value В 
then the increments in correction in each of the compo- 
nents maintains the same sign from iteration to iteration 
while Вт moves in the general direction of В. But as Bm 
enters the neighborhood of В the signs begin to oscillate 
indicating that 85 is repeatedly overshooting the true 
value В. The increments of allowable correction are 
then decreased so that the estimate В» converges to В. 
Using the signs of the corrections is one method for 
accomplishing this end. 

The sequences dx and dy, are used to sense the 
gradient of J(8m). To avoid being diverted into insignif- 
icant local minima the process starts out with rather 
large values (nominally 400 ft.) of ах, and dym. But as 
Bm begins to converge to £ these values are decreased 
so that they more accurately represent the gradient near 
B. 'The computational processor 26 uses the sequences: 


4хт= (@хо)/т m—1,2,... (24) 


апа 


dym=(dyo)/m. (25) 
These sequences are different then for cx and cy» in 
that for Bm in the neighborhood of В the ах, and ау, 
decrese much less rapidly. 

At some point in the processing of data the iterations 
in position error are terminated and a process begins to 
minimize the velocity error. The form of the correction 
term €m4+ 1 Юг velocity error corrections is very similar 
to the one for position. For velocity error: 


(26) 
0 
. 0 
ЧТ | окы зет Вт) — Jm + 9xm4- 0] 
сут 158 КВт) — Вт + Sym. 1)] 
where 
0 0 
0 0 
бхт = ах ‚ бут = 0 
0 аут 


and ст, Cm, dim, Аут are decreasing scalar sequences 
of the same form as indicated for the position algorithm. 
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Operation of the computational processor 26, as indi- 
cated by the sequence 200, is switched back and forth 
between position error estimation and velocity error 
estimation. The test completed at sequence 200 for de- 
termining which errors to estimate at this intersection is 
determined as follows: 

If 

(Xm)? + (cy)? E [km + (сут)? (КАТ (27) 
then position error estimation is performed, otherwise 
velocity error estimation is performed. The left hand 
side of the above equation (15) is the magnitude squared 
of the position correction at the mth iteration and the 
right hand side is the magnitude squared of the equiva- 
lent position correction (when applied to the last obser- 
vation) due to velocity errors. It is desirable to correct 
for position errors when such errors are dominant in 
position due to velocity and vice versa. 

Proceeding with the position error estimation, a com- 
putation 204 is made to determine perturbations that are 
to be used to obtain an estimate of the direction of the 
gradient (Вт) in the position (Ax, Ay) plane which is 
obtained in the calculation of a computation step 206. 
These perturbations are given by the expressions: 


ӧхт+1, бУт-+1. 


Following this computation the estimation of position 
error advances to a computation step 206 which com- 
pletes calculations for computing equations (15) 
through (19) as set forth previously. Next the position 
estimation error routine advances to a computation 208 
to complete the sequences of equations (22) and (23) 
which are chosen to steadily decrease so that the incre- 
ment of correction decreases. Position error is mini- 
mized by completing a computation 210 in the computa- 
tional processor of FIG. 4 to carry out the computation 
of equation (21). Each successive value of the position 
error is obtained at a computing step 212 in accordance 
with the relation (20), where the four dimensional cor- 
rection €m+1 is the result of the computation 210. This 
position error estimation is recursively carried out by 
recycling through the test 200. 

When the test of 200 as given by the expression (27), 
indicates that the next iteration is for a velocity error 
estimation, then the computational processor 26 as de- 
tailed in FIG. 4 calls instructions from memory to com- 
plete a computation 214 to determine the partial deriva- 
tive of the “x” velocity vector and the “y” velocity 
vector. These partial derivatives are given by the ex- 
pressions: 


бх b OY m1. 


Next a computation 216, which is similar to the com- 
putation 206, as given by the equations (15) to (19) is 
run. That is, the values are computed for the velocity 
error estimation. Next a computation 218 is run for the 
velocity vectors as given by equations (22) and (23), 
which is the magnitude for the correction at the mth 
iteration. A correction term is then computed for the 
velocity error at step 220 which computation is given 
by the equation (26). This error correction value is 
utilized at a computation 22 to give a velocity error 
estimate as given by the expression (20). This is the same 
expression that is run during the computation 212. 

Upon completion of the computation 222 a new esti- 
mate of the velocity error has been generated and the 
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14 
sequence of operation of the computational processor 
26, as illustrated in FIG. 4, returns to the stop inquiry 
198. 

The operation of the processor of FIG. 4 continues 
cycling through the position loop and the velocity loop 
to provide new estimates of position error and velocity 
error. These estimates are output from the estimator 30 
and stored in the data register 32 for access by the iner- 
tial navigation system 18. 

In the above described implementation of the present 
invention, there is provided a navigational system 
which possesses both operational flexibility and the 
ability to perform satisfactorily where adverse environ- 
mental and/or flight conditions exist. Furthermore, the 
device is simple, accurate, reliable, compact and light- 
weight. 

While only one embodiment of the invention has been 
described in detail herein and shown in the accompany- 
ing drawings, it will be evident that various further 
modifications are possible in the arrangement and con- 
struction of the components without departing from the 
scope of the invention. 

І claim: 

1. Apparatus for determining the position and veloc- 
ity of a vehicle independent of the nature of the vehicle 
path over a given terrain, comprising: 

a navigation system generating output data represent- 
ing the measured position, altitude and vertical 
velocity of the vehicle, 

terrain clearance detector means receiving the vehi- 
cle altitude and vertical velocity outputs and gen- 
erating output data representing the observed vehi- 
cle clearance above the terrain along the vehicle 
path, 

means for storing data representing a terrain eleva- 
tion map as a mathematical function, 

а computational processor responsive to the position 
data from said navigation system, output data from 
said terrain clearance detector means, and data 
representing the terrain elevation map to generate 
correction vehicle velocity and vehicle position 
signals, and 

means for inputting the correction signals to said 
navigation system to generate corrected vehicle 
navigation signals. 

2. Apparatus for determining the position and veloc- 
ity of a vehicle as set forth in claim 1 wherein said 
detector means includes a leading edge estimator for 
generating a time of arrival signal from a terrain clear- 
ance signal. 

3. Apparatus for determining the position and veloc- 
ity of a vehicle as set forth in claim 1 wherein said 
terrain clearance detector means includes means for 
estimating the time of arrival of a return pulse reflected 
from the terrain along the vehicle path. 

4. Apparatus for determining the position and veloc- 
ity of a vehicle independent of the nature of the vehicle 
path over a given terrain, comprising: 

a navigation system generating output data represent- 
ing the measured position and velocity of the vehi- 
cle, 

terrain clearance detector means responsive to the 
output data of said navigation system for generat- 
ing output data representing the observed vehicle 
clearance above the terrain along the vehicle path, 

means for storing a table of coefficients of the sum of 
an orthogonal function to compute a terrain eleva- 
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tion map by the expression hz(x,y), where x and у 
are rectangular coordinates of stored data, 

a computational processor responsive to the output 
data from said navigation system, output data from 
said terrain clearance detector means, and data 
representing the terrain elevation map to generate 
correction vehicle velocity and vehicle position 
signals, and 

means for inputting the correction signals to said 
navigation system to generate corrected vehicle 
navigation signals. 

5. Apparatus for determining the position and veloc- 
ity of a vehicle as set forth in claim 4 wherein said 
computational processor includes means for interpolat- 
ing the stored terrain elevation map to generate the 
correction vehicle velocity and vehicle position signals 
for any location of the map area. 

6. Apparatus for determining the position and veloc- 
ity of a vehicle as set forth in claims 4 or 5 wherein the 
means for storing includes means for storing a table of 
cosine functions for computation of the terrain eleva- 
tion map. 

7. Apparatus for determining the position and veloc- 
ity of a vehicle independent of the nature of the vehicle 
path over a given terrain, comprising: 
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a navigation system generating output data represent- 
ing the measured position and velocity of the vehi- 
cle, 

terrain clearance detector means responsive to output 
data from the navigation system to generate output 
data representing the observed vehicle clearance 
over the terrain along the vehicle path, 

means for storing as a mathematical function a repre- 
sentation of a terrain elevation map, 

a computational processor responsive to the output 
data from said navigation system, output data from 
said terrain clearance detector means, and data 
representing the terrain elevation map to generate 
three-dimensional vehicle velocity correction sig- 
nals and three-dimensional vehicle position correc- 
tion signals, and 

means for inputting the three dimensional velocity 
correction signals and the three-dimensional vehi- 
cle position correction signals to said navigation 
system to generate corrected. vehicle navigation 
signals. 

8. Apparatus for determining the position and veloc- 
ity of a vehicle as set forth in claim 7 wherein said 
means for inputting includes storage means for storing 
the correction signals for access by the navigation sys- 


tem. 
* * * * * 


